% (C) Hyewon SEO, 2011
% 2D triangle based strain analysis, based on Mohr's circle
%
% e_1: maximum principal strain value
% e_2: minimum principal strain value
% p1: principal strain direction
%
% v1, v2, v3: coord. of a triangle before the deformation
% v1_, v2_, v3_: coord. of a triangle after the deformation
%
%
% Usage: 
%{
 v1 =  [0  0 0]'
 v2 =  [5  0 0]'
 v3 =  [0  10 0]'
 
 v1_ = [0.5 0.1 0]'
 v2_ = [4.5 5 0]'
 v3_ = [0.5 9.5 0]'
 tri2D_strain(v1, v2, v3, v1_, v2_, v3_)
%}
function [e_1, e_2, p1_] = tri2D_strain(v1, v2, v3, v1_, v2_, v3_)
% Compute the normal vector of the plane of triangle v1,v2,v3
e1 = v2 - v1;
e2 = v3 - v1;
n = cross(e1, e2);
n = n / norm(n);     % Normalize the length of the normal vector

% Compute the normal vector of the plane of triangle v1_,v2_,v3_
e1_ = v2_ - v1_;
e2_ = v3_ - v1_;
n_ = cross(e1_, e2_);
n_ = n_ / norm(n_);     % Normalize the length of the normal vector

% Now compute the rotation(in angle-axis form) between the two normal vectors
theta = acos(dot(n,n_));
theta = -theta;
axis = cross(n, n_);
if(norm(axis)~=0)
    axis = axis / norm(axis);
    cth = cos(theta/2);
    sth = sin(theta/2);
    quat = quaternion(cth, sth*axis(1), sth*axis(2), sth*axis(3));
    % Test code
    qn_ = quaternion (n_(1), n_(2), n_(3));
    qn_ = quat * qn_ * conj(quat);

    % Rotate the triangle v1_,v2_,v3_ so that it becomes parallel to
    % triangle v1,v2,v3
    qv1_ = quaternion(v1_(1), v1_(2), v1_(3));
    w1_ = quat * qv1_ * conj(quat);
    qv2_ = quaternion(v2_(1), v2_(2), v2_(3));
    w2_ = quat * qv2_ * conj(quat);
    qv3_ = quaternion(v3_(1), v3_(2), v3_(3));
    w3_ = quat * qv3_ * conj(quat);
    
    % Now the w1_, w2_, and w3_ are on planes of same orientation as v1, v2, and v3.
    w1_ = [x(w1_); y(w1_); z(w1_)];
    w2_ = [x(w2_); y(w2_); z(w2_)];
    w3_ = [x(w3_); y(w3_); z(w3_)];
    
    % Reshape w's, so that matrix dimensions agree
    w1_ = reshape(w1_, [1 1 3]);
    w2_ = reshape(w2_, [1, 1, 3]);
    w3_ = reshape(w3_, [1, 1, 3]);
else
    w1_ = v1_;
    w2_ = v2_;
    w3_ = v3_;
end

% Compute new unit vectors e1, e2, and e3 using v1, v2, and n
ex = e1 / norm(e1);
ey = cross(n, ex);
ez = n;


% New coordinates using the new unit vectors
w1 = [dot(v1, ex); dot(v1, ey); dot(v1, ez)];
w2 = [dot(v2, ex); dot(v2, ey); dot(v2, ez)];
w3 = [dot(v3, ex); dot(v3, ey); dot(v3, ez)];
w1_ = [dot(w1_, ex); dot(w1_, ey); dot(w1_, ez)];
w2_ = [dot(w2_, ex); dot(w2_, ey); dot(w2_, ez)];
w3_ = [dot(w3_, ex); dot(w3_, ey); dot(w3_, ez)];

% Translate the post-deformtion triangle to the center of the
% pre-deformation triangle
delta = ((w1_+w2_+w3_)-(w1+w2+w3))/3;
w1_= w1_ - delta;
w2_= w2_ - delta;
w3_= w3_ - delta;

% Draw the figure
verts = [w1(1:2)';w2(1:2)';w3(1:2)';w1_(1:2)';w2_(1:2)';w3_(1:2)'];
faces = [1 2 3; 4 5 6];
color = [0 0 1];
% patch('Vertices',verts, 'Faces', faces, 'FaceColor', color)

% Computation of the strains (normal and shear)
q1 = w1_(1) - w1(1);
q2 = w1_(2) - w1(2);
q3 = w2_(1) - w2(1);
q4 = w2_(2) - w2(2);
q5 = w3_(1) - w3(1);
q6 = w3_(2) - w3(2);

J = [w1(1)-w3(1) w1(2)-w3(2);
     w2(1)-w3(1) w2(2)-w3(2)];
 
y23 = w2(2)-w3(2);
y13 = w1(2)-w3(2);
x23 = w2(1)-w3(1);
x13 = w1(1)-w3(1);

detJ = det(J);

if(detJ == 0) 
    Area = polyarea([w1(1) w2(1) w3(1)], [w1(2) w2(2) w3(2)]);
    detJ = 2*Area;
end

eps = [y23*(q1-q5)-y13*(q3-q5);
      -x23*(q2-q6)+x13*(q4-q6);
      -x23*(q1-q5)+x13*(q3-q5)+y23*(q2-q6)-y13*(q4-q6)] / detJ;

% Computation of Mohr's circle
e_x = eps(1);
e_y = eps(2);
e_xy = eps(3)/2;

R = 0.5 * sqrt(power(e_xy,2) + power(e_x- e_y, 2));
e_1 = (e_x+e_y)*0.5 + R;
e_2 = (e_x+e_y)*0.5 - R;
if(e_x == e_y)
    phi = 90/180*3.14;
else
    if (e_xy~=0)
        phi = 0.5 * atan(e_xy/(abs(e_x - e_y)));
    else    % if e_xy == 0
        phi = 90/180*3.14;
    end
end

% Having computed phi, we can compute e1 by rotating 'e_A_' by phi.
p1 = RotVecArAxe(ex,ez,phi);

% Finally we must compute 'p1' back in the orignial coordinate system.
% The rotation angle is computed from the z axis and the n of the triangle plane. 
% z = [0 0 1]'
% cthe = dot(z,n) 
% sthe = cross(z,n)
% if(norm(sthe) ~= 0)
%     rot_ang = atan2(z(2), z(1)) - atan2(n(2), n(1))
%     if(rot_ang < 0)
%         rot_ang = rot_ang + 2 * 3.14
%     end
%     rot_axe = sthe
%     p1_ = RotVecArAxe(p1,rot_axe,rot_ang)
% else
%     p1_ = p1
% end
% 
center = (v1+v2+v3) / 3;

% Reshape
center = reshape(center, [3 1]);
p1_ = p1 + center;
%plot3 (center(1),center(2),center(3), p1_(1), p1_(2), p1_(3))

% print maximum strain value
%e_1
% print minimum strain value
%e_2


